//testing 2 VL53L1X on Final Project board //Call necessary libraries #include #include //Define Sensors VL53L1X Left; VL53L1X Middle; VL53L1X Right; //VL53L1X Extra; //Define ATtiny3216 pin variables //VCC on pin 1 int PWMA = 0; //PWMA on pin 2 int PWMB = 1; //PWMB on pin 3 int INA1 = 2; //INA1 on pin 4 int INA2 = 3; //INA2 on pin 5 int INB1 = 4; //INB1 on pin 6 int INB2 = 5; //INB2 on pin 7 //ATtiny3216 RX on pin 8 (6 Arduino designation) //ATtiny3216 TX on pin 9 (7 Arduino designation) //ATtiny3216 SDA on pin 10 (8 Arduino designation) //ATtiny3216 SCL on pin 11 (9 Arduino designation) int XSHUT_L = 10; //Left XSHUT on pin 12 int XSHUT_M = 11; //Middle XSHUT on pin 13 int XSHUT_R = 12; //Right XSHUT on pin 14 int XSHUT_E = 13; //Extra XSHUT on pin 15 //pin 16 (17 Arduino designation) is the UPDI pin //pin 17 (14 Arduino designaion)is not connected to anything //pin 18 (15 Arduino designaion)is not connected to anything int LED = 16; //LED on pin 19 //GND on pin 20 //Define other varialbles int velocityA = 100; //set motor A speed between 0-255 int velocityB = 100; //set motor B speed between 0-255 int velocityS = 75; //set steering speed int distL = 0; //initialize distance reading on left ToF sensor int distM = 0; //initialize distance reading on middle ToF sensor int distR = 0; //initialize distance reading on Right ToF sensor int minDist = 300; int maxDist = 1000; int steerDist = 200; void setup() { //setup pin modes pinMode(PWMA, OUTPUT); pinMode(PWMB, OUTPUT); pinMode(INA1, OUTPUT); pinMode(INA2, OUTPUT); pinMode(INB1, OUTPUT); pinMode(INB2, OUTPUT); pinMode(XSHUT_L, OUTPUT); pinMode(XSHUT_M, OUTPUT); pinMode(XSHUT_R, OUTPUT); //pinMode(XSHUT_E, OUTPUT); pinMode(LED, OUTPUT); //set all XSHUT pins to LOW digitalWrite(XSHUT_L, LOW); digitalWrite(XSHUT_M, LOW); digitalWrite(XSHUT_R, LOW); //digitalWrite(XSHUT_E, LOW); //Initialize I2C delay(500); Wire.begin(); Wire.beginTransmission(0x29); Wire.setClock(400000); //use 400 kHz for I2C Serial.begin(115200); //set address of left sensor digitalWrite(XSHUT_L, HIGH); delay(150); Serial.println("00"); Left.init(); Serial.println("01"); delay(100); Left.setAddress(0x31); Serial.println("02"); //set address of middle Sensor digitalWrite(XSHUT_M, HIGH); delay(150); Serial.println("03"); Middle.init(); Serial.println("04"); delay(100); Middle.setAddress(0x33); Serial.println("05"); //set address of middle Sensor digitalWrite(XSHUT_R, HIGH); delay(150); Serial.println("06"); Right.init(); Serial.println("07"); delay(100); Right.setAddress(0x35); Serial.println("08"); // //set address of extra Sensor // digitalWrite(XSHUT_E, HIGH); // delay(150); // Serial.println("09"); // Extra.init(); // Serial.println("10"); // delay(100); // Extra.setAddress(0x37); // Serial.println("11"); //Distance modes: //Short: 1360 mm in dark/1350 mm in light, Time budget min = 20 ms //Medium: 2900 mm in dark / 760 mm in light, Time budget min = 33 ms //Long: 3600 mm in dark / 730 mm in light, Time budget min = 50 ms (140ms for 4m) //setup Left sensor Left.setDistanceMode(VL53L1X::Medium); Left.setMeasurementTimingBudget(33000); Left.startContinuous(33); Left.setTimeout(100); //setup Middle sensor Middle.setDistanceMode(VL53L1X::Long); Middle.setMeasurementTimingBudget(50000); Middle.startContinuous(50); Middle.setTimeout(100); //setup Right sensor Right.setDistanceMode(VL53L1X::Medium); Right.setMeasurementTimingBudget(33000); Right.startContinuous(33); Right.setTimeout(100); // //setup Extra sensor // Extra.setDistanceMode(VL53L1X::Long); // Extra.setMeasurementTimingBudget(50000); // Extra.startContinuous(50); // Extra.setTimeout(100); delay(150); Serial.println("addresses set"); Serial.println("I2C scanner. Scanning ..."); byte count = 0; for (byte i = 1; i < 120; i++) { Wire.beginTransmission (i); if (Wire.endTransmission () == 0) { Serial.print("Found address: "); Serial.print(i, DEC); Serial.print(" (0x"); Serial.print(i, HEX); Serial.println(")"); count++; delay(1); } //end of good response } //end of for loop Serial.println("Done."); Serial.print("Found "); Serial.print(count, DEC); Serial.println(" device(s)."); digitalWrite(LED, HIGH); } //end of void setup() void loop() { // //Print readings on Serial Monitor // //read Left Sensor // Serial.print("Left sensor: "); // Serial.print(Left.read()); // if (Left.timeoutOccurred()) {Serial.print("Timeout");} // Serial.println(); // delay(500); // //read Middle Sensor // Serial.print("Middle sensor: "); // Serial.print(Middle.read()); // if (Middle.timeoutOccurred()) {Serial.print("Timeout");} // Serial.println(); // delay(500); // //read Right Sensor // Serial.print("Right sensor: "); // Serial.print(Right.read()); // if (Right.timeoutOccurred()) {Serial.print("Timeout");} // Serial.println(); // delay(500); // //read Extra Sensor // Serial.print("Extra sensor: "); // Serial.print(Extra.read()); // if (Extra.timeoutOccurred()) {Serial.print("Timeout");} // Serial.println(); // delay(500); //Read and store distance measurements in varialbles //Left ToF sensor Left.read(); distL = Left.ranging_data.range_mm; //Middle ToF sensor Middle.read(); distM = Middle.ranging_data.range_mm; //Right ToF sensor Right.read(); distR = Right.ranging_data.range_mm; //-------------Motor control settings-------------// //towards me: INA1-LOW & INA2-HIGH / INB1-LOW & INB2-HIGH //away from me: INA1-HIGH & INA2-LOW / INB1-HIGH & INB2-LOW //break: INA1-HIGH & INA2-HIGH / INB1-HIGH & INB2-HIGH //coast: INA1-LOW & INA2-LOW / INB1-LOW & INB2-LOW //IF too far, come towards me if ( distM > maxDist) { //power on motor A digitalWrite(INA1,LOW); digitalWrite(INA2,HIGH); analogWrite(PWMA,velocityA); //power on motor B digitalWrite(INB1,LOW); digitalWrite(INB2,HIGH); analogWrite(PWMB,velocityB); } //IF too close, move away from me if ( distM < minDist) { //power on motor A digitalWrite(INA1,HIGH); digitalWrite(INA2,LOW); analogWrite(PWMA,velocityA); //power on motor B digitalWrite(INB1,HIGH); digitalWrite(INB2,LOW); analogWrite(PWMB,velocityB); } //IF reasonable distance, break // else {// if ( (distM > minDist) && (distM < maxDist) ) { //break motor A digitalWrite(INA1,HIGH); digitalWrite(INA2,HIGH); analogWrite(PWMA,velocityA); //break motor B digitalWrite(INB1,HIGH); digitalWrite(INB2,HIGH); analogWrite(PWMB,velocityB); } if ( (distM > minDist) && (distM < maxDist) && (distL < steerDist) ) { //break motor A digitalWrite(INA1,HIGH); digitalWrite(INA2,HIGH); analogWrite(PWMA,velocityA); //power on motor B away from me digitalWrite(INB1,HIGH); digitalWrite(INB2,LOW); analogWrite(PWMB,velocityS); } if ( (distM > minDist) && (distM < maxDist) && (distR < steerDist) ) { //power on motor A away from me digitalWrite(INA1,HIGH); digitalWrite(INA2,LOW); analogWrite(PWMA,velocityS); //break motor B digitalWrite(INB1,HIGH); digitalWrite(INB2,HIGH); analogWrite(PWMB,velocityS); } } //end of void loop()